#include "mainwindow.h"
#include "ui_mainwindow.h"

MainWindow::MainWindow(QWidget *parent, int argc, char** argv)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    rclcpp::init(argc, argv);
    node_ = rclcpp::Node::make_shared("ControlInterfaceNode");
    
    // 初始化ROS发布者
    target_pos_pub_ = node_->create_publisher<custom_msgs::msg::GpsDataArray>("/target_global_position", 10);

    init_QProcess();
    init_Variable();
    init_GUI();
    init_map();

    startBoatCamOrigin();
    startBoatCamYolo();
    startDockCam();
    startDockCamOrigin();
    startDockCamYolo();
    startGoDock();
    startPositionCtrl();
}

void MainWindow::init_QProcess()
{
    roscoreProcess = nullptr;
    startDockProcess = nullptr;
    startDockCamProcess = nullptr;
    startDockCamYoloProcess = nullptr;
    positionControlProcess = nullptr;
}

void MainWindow::init_Variable()
{
    course = 0.0;
}

// 主窗口初始化
void MainWindow::init_GUI()
{
    QPixmap pixmap_logo(":/images/logo_nahang.png");
    ui->label_log->setPixmap(pixmap_logo);
    ui->label_log->setScaledContents(true);

    ui->textEdit_qdebug->document()->setMaximumBlockCount(100);

    // 定时器设置，用来定期调用 rclcpp::spin_some()
    timer_spin = new QTimer(this);
    connect(timer_spin, &QTimer::timeout, this, &MainWindow::rosSpinOnce);
    timer_spin->start(50);

    connect(ui->pushButton_reset, &QPushButton::clicked, [=]() {
        ui->verticalSlider_throttle->setValue(50);
        ui->horizontalSlider_yaw->setValue(50);
    });

    init_UpdateSubscriber();
    init_map();
}

void MainWindow::init_map()
{
    webChannel = new QWebChannel(this);
    mapChannel = new MapChannel(this);
    webChannel->registerObject("passId", mapChannel);
    ui->widget_map->page()->setWebChannel(webChannel);
    
    // 获取ROS功能包路径
    // std::string package_path = ament_index_cpp::get_package_share_directory("app_interface");
    // QString mapPath = QString::fromStdString(package_path) + "/map/OnlineMap/map.html";
    QString mapPath = "/home/ubuntu/ros2_boat_ws/src/app_interface/map/OfflineMap/map.html";
    qDebug() << "Map path:" << mapPath;
    ui->widget_map->load(QUrl::fromLocalFile(mapPath));
    // ui->widget_map->setHtml("<h1>Hello World</h1>");
    
    // 将实际的经纬度信息发给地图显示
    connect(this, &MainWindow::updateBoatPosition, mapChannel, &MapChannel::updateBoatPos);
    
    // 将地图上的航点通过ROS话题发布出去，打包成custom_msgs::msg::GpsDataArray
    connect(mapChannel, &MapChannel::pointsCome, [this](int index, double lng, double lat) {
        qDebug() << "Waypoint:" << index 
        << "Longitude:" << QString::number(lng,'f',6) 
        << "Latitude:" << QString::number(lat,'f',6);
            
        // 创建单个GPS数据点
        sensor_msgs::msg::NavSatFix gps_point;
        gps_point.header.stamp = node_->now();
        gps_point.header.frame_id = "map";
        gps_point.latitude = lat;
        gps_point.longitude = lng;
        gps_point.altitude = 0.0;
        gps_point.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
        
        // 将GPS点添加到全局数组中
        target_gps_array_.gps_data.push_back(gps_point);
        
        // 设置消息头
        target_gps_array_.header.stamp = node_->now();
        target_gps_array_.header.frame_id = "map";
        
        RCLCPP_INFO(node_->get_logger(), "Added waypoint to array: lat=%.6f, lon=%.6f, total points=%zu", 
        lat, lng, target_gps_array_.gps_data.size());
    });
    // connect(mapChannel, &MapChannel::pointsCome, 
    //         [this](int index, double lng, double lat) {
    //     qDebug() << "Waypoint:" << index 
    //              << "Longitude:" << QString::number(lng,'f',6) 
    //              << "Latitude:" << QString::number(lat,'f',6);
        
    //     auto target_msg = sensor_msgs::msg::NavSatFix();
    //     target_msg.header.stamp = node_->now();
    //     target_msg.header.frame_id = "map";
        
    //     target_msg.latitude = lat;
    //     target_msg.longitude = lng;
    //     target_msg.altitude = 0.0;
        
    //     // 设置协方差
    //     target_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
        
    //     // 发布消息
    //     if (target_pos_pub_) {
    //         target_pos_pub_->publish(target_msg);
    //         RCLCPP_INFO(node_->get_logger(), "Published target position: lat=%.6f, lon=%.6f", lat, lng);
    //     } else {
    //         RCLCPP_WARN(node_->get_logger(), "ROS publisher not initialized");
    //     }
    // });
    
    // 添加ROS消息处理的定时器
    QTimer *ros_timer = new QTimer(this);
    connect(ros_timer, &QTimer::timeout, [this]() {
        if (rclcpp::ok()) {
            rclcpp::spin_some(node_);
        }
    });
    ros_timer->start(100);
}

void MainWindow::init_UpdateSubscriber()
{
    // 创建兼容mavros的QoS设置
    auto sensor_qos = rclcpp::QoS(10)
        .reliability(rclcpp::ReliabilityPolicy::BestEffort)
        .durability(rclcpp::DurabilityPolicy::Volatile)
        .history(rclcpp::HistoryPolicy::KeepLast);

    // 使用兼容的QoS设置创建订阅者
    localPoseSubscriber = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
        "/mavros/local_position/pose", sensor_qos, 
        std::bind(&MainWindow::localPoseCallback, this, std::placeholders::_1));
    
    globalPositionSubscriber = node_->create_subscription<sensor_msgs::msg::NavSatFix>(
        "/mavros/global_position/global", sensor_qos,
        std::bind(&MainWindow::globalPositionCallback, this, std::placeholders::_1));
    
    localVelocitySubscriber = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
        "/mavros/local_position/velocity_body", sensor_qos,
        std::bind(&MainWindow::localVelocityCallback, this, std::placeholders::_1));
}
// void MainWindow::init_UpdateSubscriber()
// {
//     // 更新经纬度等信息显示
//     localPoseSubscriber = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
//         "/mavros/local_position/pose", 10, 
//         std::bind(&MainWindow::localPoseCallback, this, std::placeholders::_1));
    
//     globalPositionSubscriber = node_->create_subscription<sensor_msgs::msg::NavSatFix>(
//         "/mavros/global_position/global", 10,
//         std::bind(&MainWindow::globalPositionCallback, this, std::placeholders::_1));
    
//     localVelocitySubscriber = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
//         "/mavros/local_position/velocity_body", 10,
//         std::bind(&MainWindow::localVelocityCallback, this, std::placeholders::_1));
// }

void MainWindow::rosSpinOnce()
{
    if (rclcpp::ok()) {
        rclcpp::spin_some(node_);
    }
}

// void MainWindow::startBoatCamOrigin()
// {
//     connect(ui->pushButton_cam_boat_origin, &QPushButton::toggled, [=](bool toggled_flag) {
//         if (toggled_flag) {
//             ui->pushButton_cam_boat_origin->setText("关闭无人船相机原图像");
//             // 启动 ROS 订阅
//             if (!boatCamOriginImageSubscriber) {
//                 rclcpp::QoS sensor_qos(10);
//                 sensor_qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);

//                 // 订阅 ROS 图像话题
//                 boatCamOriginImageSubscriber = node_->create_subscription<sensor_msgs::msg::CompressedImage>(
//                     "/camera/color/image_raw/compressed", sensor_qos,
//                     std::bind(&MainWindow::boatCamImageOriginCallback, this, std::placeholders::_1));
//                 ui->textEdit_qdebug->append("<font color='green'>已开始订阅相机原图像话题</font>");
//             }
//         } else {
//             ui->pushButton_cam_boat_origin->setText("开启无人船相机原图像");
//             // 取消订阅 ROS 图像话题
//             if (boatCamOriginImageSubscriber) {
//                 boatCamOriginImageSubscriber.reset();
//                 ui->textEdit_qdebug->append("<font color='red'>已停止订阅相机原图像话题</font>");
//             }
//         }
//     });
// }
void MainWindow::startBoatCamOrigin()
{
    connect(ui->pushButton_cam_boat_origin, &QPushButton::toggled, [=](bool toggled_flag) {
        if (toggled_flag) {
            ui->pushButton_cam_boat_origin->setText("关闭无人船相机原图像");
            
            if (!boatCamOriginImageSubscriber) {
                try {
                    rclcpp::QoS qos_profile_realtime(30); // 更高的发布频率，根据您的需求调整该值
                    qos_profile_realtime.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); // 可靠的数据传输
                    qos_profile_realtime.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); // 减少消息积压
                    
                    boatCamOriginImageSubscriber = node_->create_subscription<sensor_msgs::msg::CompressedImage>(
                        "/camera/color/image_raw/compressed", qos_profile_realtime,
                        std::bind(&MainWindow::boatCamImageOriginCallback, this, std::placeholders::_1));
                        
                    ui->textEdit_qdebug->append("<font color='green'>已开始订阅相机原图像话题</font>");
                } catch (const std::exception& e) {
                    ui->textEdit_qdebug->append(QString("<font color='red'>订阅失败: %1</font>").arg(e.what()));
                }
            }
        } else {
            ui->pushButton_cam_boat_origin->setText("开启无人船相机原图像");
            
            if (boatCamOriginImageSubscriber) {
                boatCamOriginImageSubscriber = nullptr;
                ui->textEdit_qdebug->append("<font color='red'>已停止订阅相机原图像话题</font>");
            }
        }
    });
}

void MainWindow::startPositionCtrl() {
    // 确保 positionControlProcess 已经初始化
    positionControlProcess = new QProcess(this);
    
    connect(ui->pushButton_position_ctrl, &QPushButton::toggled, [=](bool toggled_flag) {
        if (toggled_flag) {
            ui->pushButton_position_ctrl->setText("关闭位置控制");
            
            // // 启动ROS2 launch文件
            // QStringList arguments;
            // arguments << "launch" << "usv_controller" << "usv_controller.launch.py";
            // positionControlProcess->start("ros2", arguments);
            
            // if (positionControlProcess->waitForStarted(5000)) {  // 等待5秒启动
                // ui->textEdit_qdebug->append("<font color='green'>USV控制器节点已启动</font>");
                
                // 给节点一些时间完全初始化
                // QTimer::singleShot(2000, [this]() {
                    // 在启动进程后发布存储的经纬度信息组
                    if (target_pos_pub_ && !target_gps_array_.gps_data.empty()) {
                        target_gps_array_.header.stamp = node_->now();
                        target_pos_pub_->publish(target_gps_array_);
                        RCLCPP_INFO(node_->get_logger(), "Published GPS array with %zu waypoints", target_gps_array_.gps_data.size());
                        ui->textEdit_qdebug->append(QString("<font color='blue'>已发布%1个航点信息</font>").arg(target_gps_array_.gps_data.size()));
                    } else if (target_gps_array_.gps_data.empty()) {
                        RCLCPP_WARN(node_->get_logger(), "No waypoints to publish");
                        ui->textEdit_qdebug->append("<font color='orange'>警告：没有航点信息可发布</font>");
                    }
                // });
            // } else {
                // ui->textEdit_qdebug->append("<font color='red'>启动USV控制器节点失败</font>");
            // }
            
        } else {
            ui->pushButton_position_ctrl->setText("开启位置控制");
            mapChannel->clearWaypoints();
            
            // 清空GPS数据数组
            target_gps_array_.gps_data.clear();
            target_gps_array_.header.stamp = node_->now();
            target_gps_array_.header.frame_id = "map";
            
            RCLCPP_INFO(node_->get_logger(), "Cleared all waypoints from GPS array");
            ui->textEdit_qdebug->append("<font color='orange'>已清空所有航点信息</font>");
            
            // // 关闭ROS节点 - 改进终止流程
            // if (positionControlProcess && positionControlProcess->state() == QProcess::Running) {
            //     // 1. 尝试正常终止
            //     positionControlProcess->terminate();
                
            //     // 2. 增加等待时间并添加超时处理
            //     if (!positionControlProcess->waitForFinished(3000)) {
            //         // 3. 强制终止进程树（包括所有子进程）
            //         QProcess::execute("pkill", {"-P", QString::number(positionControlProcess->processId())});
                    
            //         // 4. 确保进程被杀死
            //         positionControlProcess->kill();
            //         positionControlProcess->waitForFinished(1000);
            //     }
                
            //     // 5. 重置进程对象
            //     positionControlProcess->close();
            //     delete positionControlProcess;
            //     positionControlProcess = nullptr;  // 重置指针
                
                ui->textEdit_qdebug->append("<font color='red'>USV控制器节点已强制关闭</font>");
            // } else {
                // ui->textEdit_qdebug->append("<font color='gray'>控制器节点未运行</font>");
            // }
        }
    });
}

void MainWindow::startBoatCamYolo()
{
    connect(ui->pushButton_cam_boat_yolo, &QPushButton::toggled, [=](bool toggled_flag) {
        if (toggled_flag) {
            ui->pushButton_cam_boat_yolo->setText("关闭无人船yolo识别图像");
            // 启动 ROS 订阅
            if (!boatCamYoloImageSubscriber) {
                // 订阅 ROS 图像话题
                boatCamYoloImageSubscriber = node_->create_subscription<sensor_msgs::msg::CompressedImage>(
                    "/yolo/predict_dock/compressed", 1,
                    std::bind(&MainWindow::boatCamImageYoloCallback, this, std::placeholders::_1));
                ui->textEdit_qdebug->append("<font color='green'>已开始订阅相机原图像话题</font>");
            }
        } else {
            ui->pushButton_cam_boat_yolo->setText("开启无人船yolo识别图像");
            // 取消订阅 ROS 图像话题
            if (boatCamYoloImageSubscriber) {
                boatCamYoloImageSubscriber.reset();
                ui->textEdit_qdebug->append("<font color='red'>已停止订阅相机原图像话题</font>");
            }
        }
    });
}

// 回调函数：显示 ROS 图像
void MainWindow::boatCamImageOriginCallback(const sensor_msgs::msg::CompressedImage::SharedPtr msg)
{
    try {
        // 解码压缩图像
        cv_bridge::CvImagePtr cv_ptr;
        cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::BGR8);
        
        // 转换颜色空间从 BGR 到 RGB (OpenCV 是 BGR, Qt 是 RGB)
        cv::Mat rgbImage;
        cv::cvtColor(cv_ptr->image, rgbImage, cv::COLOR_BGR2RGB);
        
        // 转换为 QImage
        QImage img = QImage(rgbImage.data, rgbImage.cols, rgbImage.rows,
                           rgbImage.step, QImage::Format_RGB888);

        // 将 QImage 转换为 QPixmap，并更新 QLabel 显示
        QPixmap pixmap = QPixmap::fromImage(img);
        ui->label_cam_boat_origin->setPixmap(pixmap.scaled(ui->label_cam_boat_origin->size(), Qt::KeepAspectRatio));
        ui->label_cam_boat_origin->setAlignment(Qt::AlignCenter);
    }
    catch (cv_bridge::Exception& e) {
        qDebug() << "cv_bridge exception: " << e.what();
    }
}
// void MainWindow::boatCamImageOriginCallback(const sensor_msgs::ImageConstPtr& msg)
// {
//     try {
//         // 使用 cv_bridge 将 ROS 图像转换为 OpenCV 图像
//         cv_bridge::CvImagePtr cvImagePtr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);

//         // 转换为 QImage
//         QImage img = QImage(cvImagePtr->image.data, cvImagePtr->image.cols, cvImagePtr->image.rows,
//                             cvImagePtr->image.step, QImage::Format_RGB888);

//         // 将 QImage 转换为 QPixmap，并更新 QLabel 显示
//         QPixmap pixmap = QPixmap::fromImage(img);
//         ui->label_cam_boat_origin->setPixmap(pixmap.scaled(ui->label_cam_boat_origin->size(), Qt::KeepAspectRatio));
//         ui->label_cam_boat_origin->setAlignment(Qt::AlignCenter);
//     }
//     catch (cv_bridge::Exception& e) {
//         qDebug() << "cv_bridge exception: " << e.what();
//     }
// }

// 回调函数：显示 ROS 图像
void MainWindow::boatCamImageYoloCallback(const sensor_msgs::msg::CompressedImage::SharedPtr msg)
{
    try {
        // 解码压缩图像
        cv_bridge::CvImagePtr cv_ptr;
        cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::BGR8);
        
        // 转换颜色空间从 BGR 到 RGB (OpenCV 是 BGR, Qt 是 RGB)
        cv::Mat rgbImage;
        cv::cvtColor(cv_ptr->image, rgbImage, cv::COLOR_BGR2RGB);
        
        // 转换为 QImage
        QImage img = QImage(rgbImage.data, rgbImage.cols, rgbImage.rows,
                           rgbImage.step, QImage::Format_RGB888);

        // 将 QImage 转换为 QPixmap，并更新 QLabel 显示
        QPixmap pixmap = QPixmap::fromImage(img);
        ui->label_cam_boat_yolo->setPixmap(pixmap.scaled(ui->label_cam_boat_yolo->size(), Qt::KeepAspectRatio));
        ui->label_cam_boat_yolo->setAlignment(Qt::AlignCenter);
    }
    catch (cv_bridge::Exception& e) {
        qDebug() << "cv_bridge exception: " << e.what();
    }
}
// void MainWindow::boatCamImageYoloCallback(const sensor_msgs::ImageConstPtr& msg)
// {
//     try {
//         // 使用 cv_bridge 将 ROS 图像转换为 OpenCV 图像
//         cv_bridge::CvImagePtr cvImagePtr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);

//         // 转换为 QImage
//         QImage img = QImage(cvImagePtr->image.data, cvImagePtr->image.cols, cvImagePtr->image.rows,
//                             cvImagePtr->image.step, QImage::Format_RGB888);

//         // 将 QImage 转换为 QPixmap，并更新 QLabel 显示
//         QPixmap pixmap = QPixmap::fromImage(img);
//         ui->label_cam_boat_yolo->setPixmap(pixmap.scaled(ui->label_cam_boat_yolo->size(), Qt::KeepAspectRatio));
//         ui->label_cam_boat_yolo->setAlignment(Qt::AlignCenter);
//     }
//     catch (cv_bridge::Exception& e) {
//         qDebug() << "cv_bridge exception: " << e.what();
//     }
// }

void MainWindow::startDockCam()
{
    connect(ui->pushButton_start_cam_dock, &QPushButton::toggled, [=](bool toggled_flag) {
        if (toggled_flag) {
            ui->pushButton_start_cam_dock->setText("关闭坞舱摄像头");
            // 启动 ROS 节点 (roslaunch)
            startDockCamProcess = new QProcess(this);  // 创建 QProcess 对象
            // 使用 bash 启动进程
            QString command = "bash";
            QStringList arguments;
            // 激活 conda 环境并启动 roslaunch
            QString activateEnv = "source ~/miniconda3/bin/activate rt-detr && roslaunch cv usb_cam.launch";
            arguments << "-c" << activateEnv; // 使用 -c 来传递命令
            startDockCamProcess->start(command, arguments);  // 启动 roslaunch

            // 捕获标准输出并显示到 textEdit_qdebug
            connect(startDockCamProcess, &QProcess::readyReadStandardOutput, [=]() {
                QString output = startDockCamProcess->readAllStandardOutput();
                // 去除终端控制字符（颜色编码等）
                output.remove(QRegExp("\\x1B\\[[0-9;]*[mK]"));
                QString coloredOutput = "<font color='green'>" + output + "</font>";
                ui->textEdit_qdebug->append(coloredOutput);  // 将输出追加到 textEdit_qdebug
            });

            // 捕获标准错误并显示到 textEdit_qdebug
            connect(startDockCamProcess, &QProcess::readyReadStandardError, [=]() {
                QString errorOutput = startDockCamProcess->readAllStandardError();
                // 去除终端控制字符（颜色编码等）
                errorOutput.remove(QRegExp("\\x1B\\[[0-9;]*[mK]"));
                QString coloredErrorOutput = "<font color='red'>" + errorOutput + "</font>";
                ui->textEdit_qdebug->append(coloredErrorOutput);  // 将错误输出追加到 textEdit_qdebug
            });

            // 检查 roslaunch 是否启动成功
            if (!startDockCamProcess->waitForStarted()) {
                ui->textEdit_qdebug->append("<font color='red'>启动 入坞摄像头程序 失败！</font>");
            } else {
                ui->textEdit_qdebug->append("<font color='green'>入坞摄像头程序 启动成功！</font>");
            }
        } else {
            ui->pushButton_start_cam_dock->setText("开启坞舱摄像头");
            if (startDockCamProcess && startDockCamProcess->state() == QProcess::Running) {
                startDockCamProcess->terminate();  // 优雅地停止进程
                if (!startDockCamProcess->waitForFinished(3000)) {  // 等待最多3秒
                    startDockCamProcess->kill();  // 如果进程没有正常退出，强制结束进程
                    ui->textEdit_qdebug->append("<font color='red'>入坞摄像头程序 被强制终止</font>");
                } else {
                    ui->textEdit_qdebug->append("<font color='red'>入坞摄像头程序 已停止</font>");
                }
            }
        }
    });
}

void MainWindow::startDockCamOrigin()
{
    connect(ui->pushButton_cam_dock_origin, &QPushButton::toggled, [=](bool toggled_flag) {
        if (toggled_flag) {
            ui->pushButton_cam_dock_origin->setText("关闭坞舱相机原图像");
            // 启动 ROS 订阅
            if (!dockCamOriginImageSubscriber) {
                // 订阅 ROS 图像话题
                dockCamOriginImageSubscriber = node_->create_subscription<sensor_msgs::msg::Image>(
                    "/camera_usb/color/image_raw", 1,
                    std::bind(&MainWindow::dockCamImageOriginCallback, this, std::placeholders::_1));
                ui->textEdit_qdebug->append("<font color='green'>已开始订阅相机原图像话题</font>");
            }
        } else {
            ui->pushButton_cam_dock_origin->setText("开启坞舱相机原图像");
            // 取消订阅 ROS 图像话题
            if (dockCamOriginImageSubscriber) {
                dockCamOriginImageSubscriber.reset();
                ui->textEdit_qdebug->append("<font color='red'>已停止订阅相机原图像话题</font>");
            }
        }
    });
}

void MainWindow::startDockCamYolo()
{
    connect(ui->pushButton_cam_dock_yolo, &QPushButton::toggled, [=](bool toggled_flag) {
        if (toggled_flag) {
            ui->pushButton_cam_dock_yolo->setText("关闭坞舱yolo识别图像");
            // 启动 ROS 节点 (roslaunch)
            startDockCamYoloProcess = new QProcess(this);  // 创建 QProcess 对象
            // 使用 bash 启动进程
            QString command = "bash";
            QStringList arguments;
            // 激活 conda 环境并启动 roslaunch
            QString activateEnv = "source ~/miniconda3/bin/activate rt-detr && roslaunch cv 02boat_angle.launch";
            arguments << "-c" << activateEnv; // 使用 -c 来传递命令
            startDockCamYoloProcess->start(command, arguments);

            // 捕获标准输出并显示到 textEdit_qdebug
            connect(startDockCamYoloProcess, &QProcess::readyReadStandardOutput, [=]() {
                QString output = startDockCamYoloProcess->readAllStandardOutput();
                // 去除终端控制字符（颜色编码等）
                output.remove(QRegExp("\\x1B\\[[0-9;]*[mK]"));
                QString coloredOutput = "<font color='black'>" + output + "</font>";
                ui->textEdit_qdebug->append(coloredOutput);  // 将输出追加到 textEdit_qdebug
            });

            // 捕获标准错误并显示到 textEdit_qdebug
            connect(startDockCamYoloProcess, &QProcess::readyReadStandardError, [=]() {
                QString errorOutput = startDockCamYoloProcess->readAllStandardError();
                // 去除终端控制字符（颜色编码等）
                errorOutput.remove(QRegExp("\\x1B\\[[0-9;]*[mK]"));
                QString coloredErrorOutput = "<font color='red'>" + errorOutput + "</font>";
                ui->textEdit_qdebug->append(coloredErrorOutput);  // 将错误输出追加到 textEdit_qdebug
            });

            // 检查 roslaunch 是否启动成功
            if (!startDockCamYoloProcess->waitForStarted()) {
                ui->textEdit_qdebug->append("<font color='red'>启动 yolo识别图像 失败！</font>");
            } else {
                ui->textEdit_qdebug->append("<font color='green'>yolo识别图像 启动成功！</font>");
                // 启动 ROS 订阅
                if (!dockCamYoloImageSubscriber) {
                    // 订阅 ROS 图像话题
                    dockCamYoloImageSubscriber = node_->create_subscription<sensor_msgs::msg::CompressedImage>(
                        "/yolo/predict_boat/compressed", 1,
                        std::bind(&MainWindow::dockCamImageYoloCallback, this, std::placeholders::_1));
                    ui->textEdit_qdebug->append("<font color='green'>已开始订阅相机原图像话题</font>");
                }
            }
        } else {
            ui->pushButton_cam_dock_yolo->setText("开启坞舱yolo识别图像");
            // 取消订阅 ROS 图像话题
            if (dockCamYoloImageSubscriber) {
                dockCamYoloImageSubscriber.reset();
                ui->textEdit_qdebug->append("<font color='red'>已停止订阅相机原图像话题</font>");
            }
            if (startDockCamYoloProcess && startDockCamYoloProcess->state() == QProcess::Running) {
                startDockCamYoloProcess->terminate();  // 优雅地停止进程
                if (!startDockCamYoloProcess->waitForFinished(3000)) {  // 等待最多3秒
                    startDockCamYoloProcess->kill();  // 如果进程没有正常退出，强制结束进程
                    ui->textEdit_qdebug->append("<font color='red'>yolo识别图像 被强制终止</font>");
                } else {
                    ui->textEdit_qdebug->append("<font color='red'>yolo识别图像 已停止</font>");
                }
            }
        }
    });
}

// 回调函数：显示 ROS 图像
void MainWindow::dockCamImageOriginCallback(const sensor_msgs::msg::Image::SharedPtr msg)
{
    try {
        // 使用 cv_bridge 将 ROS 图像转换为 OpenCV 图像
        cv_bridge::CvImagePtr cvImagePtr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8);

        // 转换为 QImage
        QImage img = QImage(cvImagePtr->image.data, cvImagePtr->image.cols, cvImagePtr->image.rows,
                            cvImagePtr->image.step, QImage::Format_RGB888);

        // 将 QImage 转换为 QPixmap，并更新 QLabel 显示
        QPixmap pixmap = QPixmap::fromImage(img);
        ui->label_cam_dock_origin->setPixmap(pixmap.scaled(ui->label_cam_dock_origin->size(), Qt::KeepAspectRatio));
        ui->label_cam_dock_origin->setAlignment(Qt::AlignCenter);
    }
    catch (cv_bridge::Exception& e) {
        qDebug() << "cv_bridge exception: " << e.what();
    }
}

// 回调函数：显示 ROS 图像
void MainWindow::dockCamImageYoloCallback(const sensor_msgs::msg::CompressedImage::SharedPtr msg)
{
    try {
        // 解码压缩图像
        cv_bridge::CvImagePtr cv_ptr;
        cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::BGR8);
        
        // 转换颜色空间从 BGR 到 RGB (OpenCV 是 BGR, Qt 是 RGB)
        cv::Mat rgbImage;
        cv::cvtColor(cv_ptr->image, rgbImage, cv::COLOR_BGR2RGB);
        
        // 转换为 QImage
        QImage img = QImage(rgbImage.data, rgbImage.cols, rgbImage.rows,
                           rgbImage.step, QImage::Format_RGB888);

        // 将 QImage 转换为 QPixmap，并更新 QLabel 显示
        QPixmap pixmap = QPixmap::fromImage(img);
        ui->label_cam_dock_yolo->setPixmap(pixmap.scaled(ui->label_cam_dock_yolo->size(), Qt::KeepAspectRatio));
        ui->label_cam_dock_yolo->setAlignment(Qt::AlignCenter);
    }
    catch (cv_bridge::Exception& e) {
        qDebug() << "cv_bridge exception: " << e.what();
    }
}
// void MainWindow::dockCamImageYoloCallback(const sensor_msgs::ImageConstPtr& msg)
// {
//     try {
//         // 使用 cv_bridge 将 ROS 图像转换为 OpenCV 图像
//         cv_bridge::CvImagePtr cvImagePtr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);

//         // 转换为 QImage
//         QImage img = QImage(cvImagePtr->image.data, cvImagePtr->image.cols, cvImagePtr->image.rows,
//                             cvImagePtr->image.step, QImage::Format_RGB888);

//         // 将 QImage 转换为 QPixmap，并更新 QLabel 显示
//         QPixmap pixmap = QPixmap::fromImage(img);
//         ui->label_cam_dock_yolo->setPixmap(pixmap.scaled(ui->label_cam_dock_yolo->size(), Qt::KeepAspectRatio));
//         ui->label_cam_dock_yolo->setAlignment(Qt::AlignCenter);
//     }
//     catch (cv_bridge::Exception& e) {
//         qDebug() << "cv_bridge exception: " << e.what();
//     }
// }

// 用于四元数转换的辅助函数，将四元数转换为欧拉角（roll, pitch, yaw）
void MainWindow::quaternion_to_euler(const geometry_msgs::msg::Quaternion& q, double& roll, double& pitch, double& yaw) {
    // 计算欧拉角
    double sinr_cosp = 2.0 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1.0 - 2.0 * (q.x * q.x + q.y * q.y);
    roll = atan2(sinr_cosp, cosr_cosp);

    double sinp = 2.0 * (q.w * q.y - q.z * q.x);
    if (fabs(sinp) >= 1)
        pitch = copysign(M_PI / 2, sinp);  // 夹角范围 [-pi/2, pi/2]
    else
        pitch = asin(sinp);

    double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
    yaw = atan2(siny_cosp, cosy_cosp);
}

void MainWindow::localPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
{
    // 获取位置
    const geometry_msgs::msg::Point& position = msg->pose.position;

    // 获取四元数方向
    const geometry_msgs::msg::Quaternion& orientation = msg->pose.orientation;

    // 转换四元数为欧拉角
    double roll, pitch, yaw;
    quaternion_to_euler(orientation, roll, pitch, yaw);

    // 话题"/mavros/local_position/pose"中的坐标系应该是东北天的
    // 将东北天中坐标系的角度转化为北东地坐标系中的角度，这样方便显示在罗盘上
    // 罗盘上，指向正北为0度，指向正东为90度，依次类推
    course = 90 - yaw * 180.0 / M_PI;
    if (course < 0)
        course = 360 + course;

    ui->label_ENU_X_value->setText(QString::number(position.x, 'f', 2) + "m");
    ui->label_ENU_Y_value->setText(QString::number(position.y, 'f', 2) + "m");
    ui->label_yaw_value->setText(QString::number(course, 'f', 2) + "°");

    ui->widget_attitude->setProperty("value", course);
}

void MainWindow::globalPositionCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) 
{
    double latitude = msg->latitude;    // 获取纬度
    double longitude = msg->longitude;  // 获取经度

    ui->label_latitude_value->setText(QString::number(latitude, 'f', 6) + "°");
    ui->label_longitude_value->setText(QString::number(longitude, 'f', 6) + "°");

    emit updateBoatPosition(longitude, latitude, course);
}

void MainWindow::localVelocityCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
{
    double v_body_x = msg->twist.linear.x;

    ui->label_throttle_value->setText(QString::number(v_body_x, 'f', 2) + "m/s");
    // qDebug() << v_body_x;
    ui->widget_speed->setProperty("value", v_body_x);
}

// 启动入坞相关程序
void MainWindow::startGoDock()
{
    connect(ui->pushButton_start_dock, &QPushButton::toggled, [=](bool toggled_flag) {
        if (toggled_flag) {
            ui->pushButton_start_dock->setText("停止入坞程序");
            // 启动 ROS 节点 (roslaunch)
            startDockProcess = new QProcess(this);  // 创建 QProcess 对象
            QString command = "roslaunch";
            QStringList arguments;
            arguments << "usv_run" << "007actuator_ctrl_dock_boat.launch";  // roslaunch 命令和参数
            startDockProcess->start(command, arguments);  // 启动 roslaunch

            // 捕获标准输出并显示到 textEdit_qdebug
            connect(startDockProcess, &QProcess::readyReadStandardOutput, [=]() {
                QString output = startDockProcess->readAllStandardOutput();
                // 去除终端控制字符（颜色编码等）
                output.remove(QRegExp("\\x1B\\[[0-9;]*[mK]"));
                QString coloredOutput = "<font color='green'>" + output + "</font>";
                ui->textEdit_qdebug->append(coloredOutput);  // 将输出追加到 textEdit_qdebug
            });

            // 捕获标准错误并显示到 textEdit_qdebug
            connect(startDockProcess, &QProcess::readyReadStandardError, [=]() {
                QString errorOutput = startDockProcess->readAllStandardError();
                // 去除终端控制字符（颜色编码等）
                errorOutput.remove(QRegExp("\\x1B\\[[0-9;]*[mK]"));
                QString coloredErrorOutput = "<font color='red'>" + errorOutput + "</font>";
                ui->textEdit_qdebug->append(coloredErrorOutput);  // 将错误输出追加到 textEdit_qdebug
            });

            // 检查 roslaunch 是否启动成功
            if (!startDockProcess->waitForStarted()) {
                ui->textEdit_qdebug->append("<font color='red'>启动 入坞程序 失败！</font>");
            } else {
                ui->textEdit_qdebug->append("<font color='green'>入坞程序 启动成功！</font>");
            }
        } else {
            ui->pushButton_start_dock->setText("启动入坞程序");
            if (startDockProcess && startDockProcess->state() == QProcess::Running) {
                startDockProcess->terminate();  // 优雅地停止进程
                if (!startDockProcess->waitForFinished(3000)) {  // 等待最多3秒
                    startDockProcess->kill();  // 如果进程没有正常退出，强制结束进程
                    ui->textEdit_qdebug->append("<font color='red'>入坞程序 被强制终止</font>");
                } else {
                    ui->textEdit_qdebug->append("<font color='red'>入坞程序 已停止</font>");
                }
            }
        }
        
    });
}


MainWindow::~MainWindow()
{
    delete ui;
    delete webChannel;
    delete mapChannel;
}
